/*
 *  egluesh.c
 *  e-glue
 *
 *  Created by Leonardo Dinnouti on 05/08/09.
 *  Copyright 2009 Home. All rights reserved.
 *
 */

#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <syslog.h>

// opencv includes
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>

// klt includes
#include "pnmio.h"
#include "klt.h"

// e-glue includes
#include "egluesh.h"
#include "egdebug.h"

// sift includes
#include "sift.h"
#include "imgfeatures.h"
#include "kdtree.h"
#include "utils.h"
#include "xform.h"

void PrintMat(CvMat *A);
void egSetIdentity(CvMat *M);

extern eg_bool_t (*egFindTransform)(CvMat **H, IplImage *img1, IplImage *img2);
extern long int timetable[MAX_TIME_TABLE_INDEX];

#ifdef CVHIGHGUI
const char  * WINDOW_NAME  = "e-glue";
#endif

int egGlueImage(const char* leftImage, const char* rightImage, const char* dstImage)
{
	IplImage *ipl_img1, *ipl_img2;
	IplImage *newimg = NULL;
	IplImage *img_to_glue = NULL;
	char fnameout[100];
	CvMat *H, *H_to_glue, *H_disp;
    
    EG_INF("entering egGlueImage");
    EG_INF("leftImage %s", leftImage);
    EG_INF("rightImage %s", rightImage);
    
#ifdef CVHIGHGUI
    // Uses the OpenCV window capability to show resulting image.
    cvNamedWindow (WINDOW_NAME, CV_WINDOW_AUTOSIZE);
#endif

	int n = 0;
    
	CvSize xformed_size;
	int dispx, dispy;
	IplImage *xformed1;
	
    ipl_img1 = cvLoadImage(leftImage, 1);
	if (!ipl_img1) {
		EG_ERR("Can't load img %s", leftImage);
		exit(EXIT_FAILURE);
	}
    n++;
    
	ipl_img2 = cvLoadImage(rightImage, 1);
	if (!ipl_img2) {
		EG_ERR("Can't load img %s", rightImage);
		exit(EXIT_FAILURE);
	}
    n++;
	
	img_to_glue = ipl_img1;
	H_to_glue = cvCreateMat(3, 3, CV_64FC1);
	H_disp = cvCreateMat(3, 3, CV_64FC1);
	cvSetIdentity(H_to_glue, cvRealScalar(1));
	cvSetIdentity(H_disp, cvRealScalar(1));
	while (ipl_img2) {
		egGetSizeDisp(cvGetSize(ipl_img1), cvGetSize(ipl_img1), H_disp,
                      &xformed_size.width, &xformed_size.height, &dispx, &dispy);
		xformed1 = cvCreateImage(xformed_size, IPL_DEPTH_8U, 3);
		cvWarpPerspective(ipl_img1, xformed1, H_disp, CV_INTER_LINEAR
                          + CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
		if (egFindTransformSIFT(&H, xformed1, ipl_img2) == EG_TRUE) {
			if (egGlue(&newimg, H_disp, img_to_glue, ipl_img2, H) == EG_TRUE) {
				EG_INF("H_to_glue:\n");
				PrintMat(H_to_glue);
				printf("Image %d processed\n", n);
			} else {
				EG_INF("Cant glue images.");
			}
		} else {
			EG_INF("Can't find the affine matrix (H).");
		}
		if (xformed1) {
			cvReleaseImage(&xformed1);
		}
		if (ipl_img1 != img_to_glue) {
			cvReleaseImage(&img_to_glue);
		}
		cvReleaseImage(&ipl_img1);
		ipl_img1 = ipl_img2;
		ipl_img2 = NULL;
        if (!ipl_img2) {
            if (newimg) {
                sprintf(fnameout, "newimg.png");
                EG_INF("Newimg created: %s\n", fnameout);
                cvSaveImage(fnameout, newimg); /* Debugging. */
#ifdef CVHIGHGUI
                cvShowImage(WINDOW_NAME, newimg);
                int key;
                while (1) {
                    key = cvWaitKey (100);
                    if (key == 'q' || key == 'Q')
                        break;
                }
#endif
                cvReleaseImage(&newimg);
            } // if (newimg)
        }
	}
	showTimeTable();
    EG_INF("exiting egGlueImage()");
	return 0;
}

/**
 * @returns the matches found.
 *
 */
int egFillPts(CvPoint2D64f *pts1, CvPoint2D64f *pts2, KLT_FeatureTable ft,
              int frame, KLT_FeatureList fl) {
	int i;
	int k = 0;
	for (i = 0; i < fl->nFeatures; i++) {
		if (fl->feature[i]->val == 0) {
			printf("Feature #%d:  (%f,%f) with value of %d\n", i,
                   fl->feature[i]->x, fl->feature[i]->y, fl->feature[i]->val);
			pts1[k].x = ft->feature[i][frame]->x;
			pts1[k].y = ft->feature[i][frame]->y;
			pts2[k].x = fl->feature[i]->x;
			pts2[k].y = fl->feature[i]->y;
			k++;
		}
	}
	return k;
}

/**
 * Converts the type formats from CvMat into printable values using snprintf
 * and shows using the EG_INF macro (info debug level).
 * @param A a valid filled matrix
 */
void PrintMat(CvMat *A) {
	int i, j, tmp_i;
	char tmp[256], tmp2[256];
	EG_DEB("Entering %s", __FUNCTION__);
	for (i = 0; i < A->rows; i++) {
		switch (CV_MAT_DEPTH(A->type)) {
            case CV_32F:
            case CV_64F:
                memset(tmp, 0, 256);
                memset(tmp2, 0, 256);
                for (j = 0; j < A->cols; j++) {
                    snprintf(tmp2, 256, "%8.3f ", (float) cvGetReal2D(A, i, j));
                    tmp_i = strlen(tmp);
                    strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
                }
                EG_INF(tmp);
                break;
            case CV_8U:
            case CV_16U:
                memset(tmp, 0, 256);
                memset(tmp2, 0, 256);
                for (j = 0; j < A->cols; j++) {
                    snprintf(tmp2, 256, "%6d", (int) cvGetReal2D(A, i, j));
                    tmp_i = strlen(tmp);
                    strncat(tmp + tmp_i, tmp2, 256 - tmp_i - strlen(tmp2));
                }
                EG_INF(tmp);
                break;
            default:
                break;
		}
	}
	EG_DEB("Exiting %s", __FUNCTION__);
}

/**
 * Retrieves data from a IplImage to be used in the KLTTrackFeatures method.
 * TODO: should be nice to use a IplImage directly into KLTTrackFeatures.
 * @param ipl_img1 the input image to be converted into raw data
 * @param data the structure where to put the retrieved data
 * @param ncols the number of columns of input image
 * @param nrows the number of rows of input image
 */
void egGetData(IplImage *ipl_img1, unsigned char **data, int *ncols, int *nrows) {
	unsigned char *ipl_data = NULL;
	IplImage *tmp1 = NULL;
	CvSize size1;
	int x, y, step, i;
	tmp1 = cvCreateImage(cvGetSize(ipl_img1), IPL_DEPTH_8U, 3);
	if (!tmp1) {
		EG_ERR("Can't create tmp1 image");
		exit(1);
	}
	cvCvtColor(ipl_img1, tmp1, CV_RGB2YCrCb);
	cvGetRawData(ipl_img1, &ipl_data, &step, &size1);
	if (!ipl_data) {
		EG_ERR("Can't get ipl_data");
		exit(1);
	}
	EG_DEB("ipl_data[0]=%d size1.width=%d size1.height=%d step=%d\n",
           ipl_data[0], size1.width, size1.height, step);
	*data = malloc(size1.width * size1.height);
	if (!*data) {
		EG_ERR("Can't allocate data");
		exit(1);
	}
	i = 0;
	for (y = 0; y < size1.height; y++, ipl_data += step)
		for (x = 0; x < size1.width; x++)
			(*data)[i++] = ipl_data[x * 3];
	cvReleaseImage(&tmp1);
	*ncols = size1.width;
	*nrows = size1.height;
}

/**
 * Given the two input images (img1 and img2) computes all transformations needed to
 * align the two images. After the transformation is obtained, an arbitrary mask is
 * applied to get the point to be copied to new the new image (newimg). Actually the
 * mask only consider the most brighter pixel.
 * @param newimg the glued image, resulting from transforming the input images and copy the
 *               resulting pixels according to an arbitrary mask
 * @param H_disp a matrix with the displacement of the 2nd imagem in relation to the 1st.
 * @param img1 the "left" input image
 * @param img2 the "right" input image
 * @param H CvMat the affine transformation that takes the 2nd image over the 1st image,
 *          unless the H_disp given displacement.
 */
eg_bool_t egGlue(IplImage **newimg, CvMat *H_disp, IplImage *img1,
                 IplImage *img2, CvMat *H) {
	IplImage* xformed2, *xformed1;
	int dispx, dispy; /* x and y displacement */
	CvMat *H_trans;
	IplImage* mask;
	struct timeval t1;
	static int toblend = 0;
	char toblend_name[256];
    
	CvSize xformed_size;
	if (H) {
		tic(&t1);
		egGetSizeDisp(cvGetSize(img1), cvGetSize(img2), H, &xformed_size.width,
                      &xformed_size.height, &dispx, &dispy);
		/* resets the displacement the fit all the new image*/
		H_trans = cvCreateMat(3, 3, H->type);
		egBuildH_disp(dispx, dispy, H_disp);
		egBuildH_trans(H, H_disp, H_trans);
		EG_DEB("\nH_trans:");
		PrintMat(H_trans);
		xformed2 = cvCreateImage(xformed_size, IPL_DEPTH_8U, 3);
		xformed1 = cvCreateImage(xformed_size, IPL_DEPTH_8U, 3);
		*newimg = cvCreateImage(xformed_size, IPL_DEPTH_8U, 3);
		/* Ensure that the both images will be at the same coordinates system */
		cvWarpPerspective(img1, xformed1, H_trans, CV_INTER_LINEAR
                          + CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
		EG_DEB("After img1\n");
		cvWarpPerspective(img2, xformed2, H_disp, CV_INTER_LINEAR
                          + CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
		snprintf(toblend_name, 256, "toblend_%03d.tif", toblend++);
		cvSaveImage(toblend_name, xformed2);
		EG_DEB("After img2\n");
		mask = egComputeMask(xformed1, xformed2);
		cvCopy(xformed1, *newimg, NULL );
		cvCopy(xformed2, *newimg, mask); /* replaces only some pixels */
		cvReleaseImage(&xformed1);
		cvReleaseImage(&xformed2);
		//        cvReleaseMat( &H_disp );
		cvReleaseMat(&H_trans);
		EG_DEB("Exiting 2 %s", __FUNCTION__);
		timetable[GLUEIMAGE] += tac(t1);
		return EG_TRUE;
	} else {
		EG_DEB("Can't compute RANSAC.\n");
		EG_DEB("Exiting 3 %s", __FUNCTION__);
		return EG_FALSE;
	}
}

/**
 * Computes which points are brighter to copy to image destination. The inputs are the images already "aligned".
 * @param xformed1 the aligned "left" image.
 * @param xformed2 the aligned "right" image.
 * @returns a IplImage corresponding to the mask to be applied when copying the pixels.
 */
IplImage* egComputeMask(IplImage *xformed1, IplImage *xformed2) {
	IplImage *tmp1, *tmp2;
	EG_DEB("Entering %s", __FUNCTION__);
	IplImage *mask = cvCreateImage(cvGetSize(xformed1), IPL_DEPTH_8U, 1);
	IplImage *tmp1arr = cvCreateImage(cvGetSize(xformed1), IPL_DEPTH_8U, 1);
	IplImage *tmp2arr = cvCreateImage(cvGetSize(xformed2), IPL_DEPTH_8U, 1);
	tmp1 = cvCreateImage(cvGetSize(xformed1), IPL_DEPTH_8U, 3);
	tmp2 = cvCreateImage(cvGetSize(xformed2), IPL_DEPTH_8U, 3);
	cvCvtColor(xformed1, tmp1, CV_RGB2YCrCb);
	cvCvtColor(xformed2, tmp2, CV_RGB2YCrCb);
	cvSplit(tmp1, tmp1arr, NULL, NULL, NULL);
	cvSplit(tmp2, tmp2arr, NULL, NULL, NULL);
	cvCmp( tmp1arr, tmp2arr, mask, CV_CMP_LT );
	cvReleaseImage( &tmp1 );
	cvReleaseImage( &tmp2 );
	cvReleaseImage( &tmp1arr );
	cvReleaseImage( &tmp2arr );
	EG_DEB( "Exiting %s", __FUNCTION__);
	return mask;
}

void egGetSizeDisp(CvSize size1, CvSize size2, CvMat* H_inv, int *width,
                   int *height, int *dispx, int *dispy) {
	int i;
	int minx, miny, maxx, maxy;
	EG_DEB("Entering %s", __FUNCTION__);
	CvMat *new_dim = cvCreateMat(3, 4, H_inv->type);
	egGuessNewDim(size2, H_inv, new_dim);
	EG_DEB("\nnew_dim:");
	PrintMat(new_dim);
	minx = 0;
	miny = 0;
	maxx = size1.width;
	maxy = size1.height;
	for (i = 1; i < new_dim->cols; i++) {
		if (minx > cvmGet(new_dim, 0, i)) {
			minx = cvmGet(new_dim, 0, i);
		}
		if (maxx < cvmGet(new_dim, 0, i)) {
			maxx = cvmGet(new_dim, 0, i);
		}
		if (miny > cvmGet(new_dim, 1, i)) {
			miny = cvmGet(new_dim, 1, i);
		}
		if (maxy < cvmGet(new_dim, 1, i)) {
			maxy = cvmGet(new_dim, 1, i);
		}
	}
	*height = maxy - miny;
	*width = maxx - minx;
	*dispx = minx;
	*dispy = miny;
	cvReleaseMat(&new_dim);
	EG_DEB("height = %d\n", *height);
	EG_DEB("width = %d\n", *width);
	EG_DEB("dispx = %d\n", *dispx);
	EG_DEB("dispy = %d\n", *dispy);
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egNorm(CvMat* src, CvMat* dst) {
	int i;
	EG_DEB("Entering %s", __FUNCTION__);
	assert(src->rows == 3);
	for (i = 0; i < src->cols; i++) {
		double factor = cvmGet(src, 2, i);
		cvmSet(dst, 0, i, cvmGet(src, 0, i) / factor);
		cvmSet(dst, 1, i, cvmGet(src, 1, i) / factor);
		cvmSet(dst, 2, i, 1.0);
	}
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egNorm3x3(CvMat* src, CvMat* dst) {
	int i, j;
	double factor;
	EG_DEB("Entering %s", __FUNCTION__);
	EG_DEB("rows = %d\n cols = %d", src->rows, src->cols);
	assert(src->rows == 3 && src->cols == 3);
	factor = cvmGet(src, 2, 2);
	/* TODO: look for a matrix to scalar multiplication. */
	for (i = 0; i < src->cols; i++) {
		for (j = 0; j < src->rows; j++) {
			cvmSet(dst, j, i, cvmGet(src, j, i) / factor);
		}
	}
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egGuessNewDim(CvSize size, CvMat* H_inv, CvMat* new_dim) {
	EG_DEB("Entering %s", __FUNCTION__);
	CvMat *dim = cvCreateMat(3, 4, H_inv->type);
	cvmSet(dim, 0, 0, 0);
	cvmSet(dim, 1, 0, 0);
	cvmSet(dim, 2, 0, 1);
	cvmSet(dim, 0, 1, size.width);
	cvmSet(dim, 1, 1, 0);
	cvmSet(dim, 2, 1, 1);
	cvmSet(dim, 0, 2, 0);
	cvmSet(dim, 1, 2, size.height);
	cvmSet(dim, 2, 2, 1);
	cvmSet(dim, 0, 3, size.width);
	cvmSet(dim, 1, 3, size.height);
	cvmSet(dim, 2, 3, 1);
	cvMatMul(H_inv, dim, new_dim);
	egNorm(new_dim, new_dim);
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egBuildH_disp(int dispx, int dispy, CvMat* H_disp) {
	EG_DEB("Entering %s", __FUNCTION__);
	cvmSet(H_disp, 0, 0, 1.0);
	cvmSet(H_disp, 0, 1, 0);
	cvmSet(H_disp, 0, 2, -dispx);
	cvmSet(H_disp, 1, 0, 0);
	cvmSet(H_disp, 1, 1, 1.0);
	cvmSet(H_disp, 1, 2, -dispy);
	cvmSet(H_disp, 2, 0, 0);
	cvmSet(H_disp, 2, 1, 0);
	cvmSet(H_disp, 2, 2, 1.0);
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egBuildH_trans(CvMat* H_inv, CvMat* H_disp, CvMat* H_trans) {
	EG_DEB("Entering %s", __FUNCTION__);
	cvMatMul(H_inv, H_disp, H_trans);
	egNorm3x3(H_trans, H_trans);
	EG_DEB("Exiting %s", __FUNCTION__);
}

void egDrawLines(IplImage *stacked, CvPoint2D64f *pts1, CvPoint2D64f *pts2,
                 int npts) {
	CvPoint pt1, pt2;
	int i;
	int height = stacked->height / 2;
	for (i = 0; i < npts; i++) {
		pt1 = cvPoint(cvRound(pts1[i].x), cvRound(pts1[i].y));
		pt2 = cvPoint(cvRound(pts2[i].x), cvRound(pts2[i].y + height));
		cvLine(stacked, pt1, pt2, CV_RGB(255, 255, 0), 1, 8, 0);
	}
}

void egShowTc(KLT_TrackingContext tc) {
	printf("KLT_TrackingContext\n"); /* minimum distance between selected features */
	printf("mindist                =\t%d\n", tc->mindist);
	printf("window_width           =\t%d\n", tc->window_width); /* dimensions of feature window */
	printf("window_height          =\t%d\n", tc->window_height);
	printf("sequentialMode         =\t%d\n", tc->sequentialMode); /* whether to save most recent image */
	printf("smoothBeforeSelecting  =\t%d\n", tc->smoothBeforeSelecting); /* whether to smooth image before selecting features */
	printf("writeInternalImages    =\t%d\n", tc->writeInternalImages); /* whether to write internal images for later viewing */
	printf("min_eigenvalue         =\t%d\n", tc->min_eigenvalue); /* smallest eigenvalue allowed for selecting */
	printf("min_determinant        =\t%f\n", tc->min_determinant); /* min determinant for declaring tracking failure */
	printf("min_displacement       =\t%f\n", tc->min_displacement); /* amount of pixel shift for stopping tracking iterations */
	printf("max_iterations         =\t%d\n", tc->max_iterations); /* max iterations before declaring tracking failure */
	printf("max_residue            =\t%f\n", tc->max_residue); /* max residue before declaring tracking failure */
	printf("grad_sigma             =\t%f\n", tc->grad_sigma); /* sigma of gaussian for computing gradient */
	printf("smooth_sigma_fact      =\t%f\n", tc->smooth_sigma_fact); /* sigma factor of gaussian for smoothing image */
	printf("pyramid_sigma_fact     =\t%f\n", tc->pyramid_sigma_fact); /* sigma factor of gaussian for computing image pyramid */
	printf("nSkippedPixels         =\t%d\n", tc->nSkippedPixels); /* used to speed up feature selection */
	printf("borderx                =\t%d\n", tc->borderx); /* border in which features will not be selected, and */
	printf("bordery                =\t%d\n", tc->bordery); /*    tracked features will be declared out-of-bounds */
	printf("nPyramidLevels         =\t%d\n", tc->nPyramidLevels); /* number of pyramid levels */
	printf("subsampling            =\t%d\n", tc->subsampling); /* amount of subsampling between pyramid levels */
}

int egFindMatches(CvPoint2D64f *matched1, CvPoint2D64f *matched2,
                  KLT_FeatureTable ft, int frame, KLT_FeatureList fl) {
	int num;
	CvMat *status;
	CvMat *points1, *points2;
	int i, k = 0;
	CvMat *fundamental_matrix;
    
	k = egFillPtsMat(&points1, &points2, ft, frame, fl);
	if (k <= 4) {
		EG_INF("Can't compute enough matches.");
		return k;
	}
	EG_DEB("Total KLT matches = %d", k);
	/* Fill the points here ... */
	status = cvCreateMat(1, points1->cols, CV_32F);
	fundamental_matrix = cvCreateMat(3, 3, CV_32F);
	//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
	num = cvFindFundamentalMat(points1, points2, fundamental_matrix,
                               CV_FM_RANSAC, 0.5, 0.99, status);
	//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.2,0.99,status);
	//	num = cvFindFundamentalMat(points1,points2,fundamental_matrix,CV_FM_RANSAC,0.5,0.50,status);
	if (num == 1) {
		k = 0;
		for (i = 0; i < status->cols; i++) {
			if (cvmGet(status, 0, i) == 1.0) {
				k++;
			}
		}
		EG_DEB("Total cvFindFundamentalMat matches = %d", k);
		EG_DEB("status:");
		PrintMat(status);
		k = 0;
		for (i = 0; i < status->cols; i++) {
			if (cvmGet(status, 0, i) == 1.0) {
				matched1[k].x = cvmGet(points1, 0, i);
				matched1[k].y = cvmGet(points1, 1, i);
				matched2[k].x = cvmGet(points2, 0, i);
				matched2[k].y = cvmGet(points2, 1, i);
				EG_DEB("matched1[%d]=%f,%f", k, matched1[k].x, matched1[k].y);
				EG_DEB("matched2[%d]=%f,%f", k, matched2[k].x, matched2[k].y);
				k++;
			}
		}
	} else {
		k = 0;
		EG_INF("Fundamental matrix was not found\n");
	}
	return k;
}

/**
 * @returns the matches found.
 *
 */
int egFillPtsMat(CvMat **pts1, CvMat **pts2, KLT_FeatureTable ft, int frame,
                 KLT_FeatureList fl) {
	int i;
	int k = 0;
	for (i = 0; i < fl->nFeatures; i++) {
		if (fl->feature[i]->val == 0) {
			k++;
		}
	}
	*pts1 = cvCreateMat(3, k, CV_32FC1);
	*pts2 = cvCreateMat(3, k, CV_32FC1);
	k = 0;
	for (i = 0; i < fl->nFeatures; i++) {
		if (fl->feature[i]->val == 0) {
			cvmSet(*pts1, 0, k, ft->feature[i][frame]->x);
			cvmSet(*pts1, 1, k, ft->feature[i][frame]->y);
			cvmSet(*pts1, 2, k, 1);
			cvmSet(*pts2, 0, k, fl->feature[i]->x);
			cvmSet(*pts2, 1, k, fl->feature[i]->y);
			cvmSet(*pts2, 2, k, 1);
			k++;
		}
	}
	return k;
}

/**
 * Returns in H a transformation matrix (4x4) representing the transform that takes
 * the img1 to same position as img2. The similarity between the images are get using
 * Kanade-Lucas-Tomasi method. This method is effective only when images are close
 * each other and with some "corners" and others precise features to be tracked. This
 * algorithm predicts the use a image sequence and, then, new features should be
 * inserted as new images are processed. This function doesn't implement this feature.
 * TODO: investigate how to implement the "replace" tracking points.
 * @param H matrix to be filled with the homogeneous transform.
 * @param img1 The first image to be compared (source)
 * @param img2 The second image to be compared (target)
 */
eg_bool_t egFindTransformKLT(CvMat **H, IplImage *img1, IplImage *img2) {
	KLT_TrackingContext tc;
	KLT_FeatureList fl;
	KLT_FeatureTable ft;
#ifdef DEBUG
	IplImage *stacked;
#endif
	unsigned char *ipl_img1_data;
	unsigned char *ipl_img2_data;
	int nFeatures = NFEATURES, nFrames = 2;
	CvPoint2D64f pts1[NFEATURES], pts2[NFEATURES];
	int ncols, nrows;
	int npts = 0;
    
	tc = KLTCreateTrackingContext();
	tc->mindist = 10; /* default 10 */
	//  egShowTc(tc);
	fl = KLTCreateFeatureList(nFeatures);
	ft = KLTCreateFeatureTable(nFrames, nFeatures);
	tc->sequentialMode = TRUE;
	tc->writeInternalImages = FALSE;
	//    tc->affineConsistencyCheck = -1;  /* set this to 2 to turn on affine consistency check */
	tc->affineConsistencyCheck = 2; /* set this to 2 to turn on affine consistency check */
    
	egGetData(img1, &ipl_img1_data, &ncols, &nrows);
	if (!ipl_img1_data) {
		EG_ERR("unable to get data from img1");
		exit(1);
	}
    
	KLTSelectGoodFeatures(tc, ipl_img1_data, ncols, nrows, fl);
	KLTStoreFeatureList(fl, ft, 0);
	KLTWriteFeatureListToPPM(fl, ipl_img1_data, ncols, nrows, "feat001.ppm");
    
	egGetData(img2, &ipl_img2_data, &ncols, &nrows);
	if (!ipl_img2_data) {
		EG_ERR("unable to get data from img2");
		exit(1);
	}
    
	KLTTrackFeatures(tc, ipl_img1_data, ipl_img2_data, ncols, nrows, fl);
    
	EG_DEB("nFeatures=%d", fl->nFeatures);
	npts = egFindMatches(pts1, pts2, ft, 0, fl);
	EG_DEB("npts=%d", npts);
	if (npts <= 4) {
		free(ipl_img1_data);
		free(ipl_img2_data);
		EG_INF("Can't compute fundamental matches.");
		return EG_FALSE;
	}
    
	*H = lsq_homog(pts1, pts2, npts);
	if (NULL != H) {
		EG_INF("Affine:");
		PrintMat(*H);
	} else {
		fprintf(stderr,"Can't find lsq_homog in %d pts.\n", npts);
	}
    
#ifdef DEBUG
	stacked = stack_imgs( img1, img2 );
	/* Debugging. */
	egDrawLines(stacked, pts1, pts2, npts);
	cvNamedWindow( "Xformed", 1 );
	cvShowImage( "Xformed", stacked );
	cvSaveImage("stacked.png", stacked);
	cvWaitKey( 0 );
#endif
    
	//#ifdef REPLACE
	//    KLTReplaceLostFeatures(tc, ipl_img2_data, ncols, nrows, fl);
	//#endif
	//
	//    KLTStoreFeatureList(fl, ft, i-1);
	//    sprintf(fnameout, "feat%03d.ppm", i);
	//    KLTWriteFeatureListToPPM(fl, ipl_img2_data, ncols, nrows, fnameout);
	//    cvReleaseImage(&ipl_img2);
	//    ipl_img2_data = NULL;
	//
	KLTFreeFeatureTable(ft);
	KLTFreeFeatureList(fl);
	KLTFreeTrackingContext(tc);
	free(ipl_img1_data);
	free(ipl_img2_data);
	return EG_TRUE;
}

/**
 * Find features based on two input images.
 *
 * Note that this line:
 *
 * feat1[i].fwd_match = nbrs[0];
 *
 * is important for the RANSAC function to work.
 *
 */
int egFindFeatures(IplImage *img1, feature_t** feat1, int *n1, IplImage *img2,
                   feature_t **feat2) {
#ifdef DEBUG
	IplImage *stacked;
#endif
	feature_t *feat;
	struct kd_node* kd_root;
	double d0, d1;
	feature_t **nbrs;
	CvPoint pt1, pt2;
	int i, k, m = 0;
	int n2;
	struct timeval t1, t2;
	tic(&t2);
	EG_DEB("Entering %s", __FUNCTION__);
#ifdef DEBUG
	stacked = stack_imgs( img1, img2 );
#endif
	tic(&t1);
	*n1 = _sift_features(img1, feat1, SIFT_INTVLS, SIFT_SIGMA,
                         EG_SIFT_CONTR_THR, EG_SIFT_CURV_THR,
                         SIFT_IMG_DBL, SIFT_DESCR_WIDTH, SIFT_DESCR_HIST_BINS );
	timetable[SIFT1] += tac(t1);
	tic(&t1);
	n2 = _sift_features(img2, feat2, SIFT_INTVLS, SIFT_SIGMA,
                        EG_SIFT_CONTR_THR, EG_SIFT_CURV_THR,
                        SIFT_IMG_DBL, SIFT_DESCR_WIDTH, SIFT_DESCR_HIST_BINS );
	timetable[SIFT2] += tac(t1);
	tic(&t1);
	kd_root = kdtree_build(*feat2, n2);
	for (i = 0; i < *n1; i++) {
		feat = *feat1 + i;
		k = kdtree_bbf_knn(kd_root, feat, 2, &nbrs, KDTREE_BBF_MAX_NN_CHKS);
		if (k == 2) {
			d0 = descr_dist_sq(feat, nbrs[0]);
			d1 = descr_dist_sq(feat, nbrs[1]);
			if (d0 < d1 * NN_SQ_DIST_RATIO_THR) {
				pt1 = cvPoint(cvRound(feat->x), cvRound(feat->y));
				pt2 = cvPoint(cvRound(nbrs[0]->x), cvRound(nbrs[0]->y));
				pt2.y += img1->height;
#ifdef DEBUG
				cvLine( stacked, pt1, pt2, CV_RGB(255,0,255), 1, 8, 0 );
#endif
				m++;
				(*feat1)[i].fwd_match = nbrs[0];
			}
		}
		free(nbrs);
	}
	timetable[KDTREE] += tac(t1);
	EG_DEB("Found %d total matches\n", m);
#ifdef DEBUG
	//    display_big_img( stacked, "Matches" );
	cvSaveImage("stacked.png", stacked);
	//    cvWaitKey( 0 );
	cvReleaseImage( &stacked );
#endif
	EG_DEB("After release stacked\n");
	kdtree_release(kd_root);
	EG_DEB("Exiting %s", __FUNCTION__);
	timetable[FINDFEATURES] += tac(t2);
	return m;
}

/**
 * Returns in H a transformation matrix (4x4) representing the transform that takes
 * the img1 to same position as img2. The similarity between the images are get using
 * Scale Invariant Features Tracking (SIF).
 * @param H matrix to be filled with the homogeneous transform.
 * @param img1 The first image to be compared (source)
 * @param img2 The second image to be compared (target)
 */
eg_bool_t egFindTransformSIFT(CvMat **H, IplImage *img1, IplImage *img2) {
	feature_t *feat1, *feat2;
	int m, n1;
	struct timeval t1;
	m = egFindFeatures(img1, &feat1, &n1, img2, &feat2);
	tic(&t1);
	*H = ransac_xform(feat1, n1, FEATURE_FWD_MATCH, lsq_homog,
                      EG_MIN_RANSAC_MATCHES, 0.01, homog_xfer_err, 3.0, NULL, NULL );
    timetable[RANSAC] += tac(t1);
    free( feat1 );
    free( feat2 );
    if ( *H )
    {
        return EG_TRUE;
    }
    else
    {
        return EG_FALSE;
    }
}

/**
 * Set an identity matrix using a loop. TODO: look for a faster way to create a identity matrix.
 * @param M The matrix to be modified.
 */
void egSetIdentity(CvMat *M) {
	int i;
	int j;
	for (i = 0; i < M->cols; i++) {
		for (j = 0; j < M->rows; j++) {
			if (i != j) {
				cvmSet(M, j, i, 0);
			} else {
				cvmSet(M, j, i, 1);
			}
		}
	}
}
